#include "common/pid_control.h"

using namespace std;

namespace control
{
  PIDControl::PIDControl()
  {
    //RCLCPP_INFO(node->get_logger(),"PIDControl constructor Fun.");
  }

  PIDControl::~PIDControl()
  {
    //RCLCPP_INFO(node->get_logger(),"PIDControl destructor Fun.");
  }

  void PIDControl::init(const PIDConf &pid_conf)
  {
    previous_error_      = 0.0;
    previous_output_     = 0.0;
    previous_diff_       = 0.0;
    integral_            = 0.0;
    first_hit_           = false;
    integrator_enabled_  = true;
    integrator_hold_     = false;
    diff_mode_ = 1;
    integrator_saturation_high_   = 0.6;
    integrator_saturation_low_    = -0.6;
    integrator_saturation_status_ = 0;
    setPID(pid_conf);
  }

  void PIDControl::setPID(const PIDConf &pid_conf)
  {
    kp_ = pid_conf.kp;
    ki_ = pid_conf.ki;
    kd_ = pid_conf.kd;
    //RCLCPP_INFO(node->get_logger(),"kp:%f,ki:%f,kd:%f",kp_,ki_,kd_);
  }
  void PIDControl::setIntegralLimit(const double high, const double low)
  {
    integrator_saturation_high_   = high;
    integrator_saturation_low_    = low;
  }
  void PIDControl::setDiffMode(const int mode )
  {
    diff_mode_ = mode;
  }

  void PIDControl::reSet()
  {
    previous_error_  = 0.0;
    previous_output_ = 0.0;
    integral_        = 0.0;
    first_hit_       = true;
    integrator_saturation_status_ = 0;
  }

  double PIDControl::control(const double error, const double dt)
  {
    if (dt <= 0) {
      //RCLCPP_INFO(node->get_logger(),"dt:%lf <= 0, control = %lf",dt,previous_output_);
      return previous_output_;
    }

    double ans_PID = 0;
    double diff = 0;
    double new_diff = 0;
    if (first_hit_) {
      first_hit_ = false;
    }
    else{
      if(diff_mode_ == 1)
      diff = error - previous_error_;
      else if(diff_mode_ == 2)
      {
        double scale =0.05;
        new_diff = error - previous_error_;
        diff = (previous_diff_*(1-scale) + new_diff*scale);
      }
      previous_diff_ = diff;
    }

    if (!integrator_enabled_) {
      integral_ = 0;
    }
    else
    {
      integral_ += error * dt * ki_;
      // printf("pid: error:%lf,  dt:%lf, ki_:%lf.\r\n",error,dt,ki_ );
      if (!integrator_hold_ ) 
      {
        if (integral_ > integrator_saturation_high_) 
        {
          integral_ = integrator_saturation_high_;
          integrator_saturation_status_ = 1;
        }
        else if (integral_ < integrator_saturation_low_)
        {
          integral_ = integrator_saturation_low_;
          integrator_saturation_status_ = -1;
        }
        else
        {
          integrator_saturation_status_ = 0;
        }        
      }
    }
    // printf("pid: p_output:%lf,  i_output:%lf, d_output:%lf.\r\n",error *kp_,integral_,diff * kd_);
    ans_PID = error *kp_ + integral_ + diff * kd_;
    previous_error_  = error;
    previous_output_ = ans_PID;
    
    return ans_PID;
  }

  int PIDControl::integratorSaturationStatus() const
  {
    return integrator_saturation_status_;
  }

  void PIDControl::setIntegratorHold(bool hold)
  {
    integrator_hold_ = hold;
  }
}

